

#include "TimeAllocation.h"
#include <iostream>


TimeAllocation::TimeAllocation(MatrixXd Path,int d_order,double vel,double acc)
{
    m_path = Path;
    m_vel = vel;
    m_acc = acc;
    m_p_num1d = 2 * d_order;
    m_time = MatrixXd::Zero(m_path.rows() - 1,m_p_num1d);
    timeAllocation();
}

TimeAllocation::~TimeAllocation()
{
}

void TimeAllocation::updateVel(double vel)
{
    m_vel = vel;
    timeAllocation();
}

void TimeAllocation::updateAcc(double acc)
{
    m_acc = acc;
    timeAllocation();
}

int TimeAllocation::getTimeSegs() const
{
    return (m_path.rows() - 1);
}

double TimeAllocation::getTime(int i,int j) const
{
    return m_time(i,j);
}
//i为第几段segment，从0开始计数；j为多少次幂，范围1~7.
double TimeAllocation::operator()(int i,int j) const
{
    return m_time(i,j);
}

//i为第几段segment，从0开始计数；j为多少次幂，范围1~7.
void TimeAllocation::timeAllocation() 
{ 
    // The time allocation is many relative timelines but not one common timeline
    for(int i = 0; i < m_path.rows() - 1; i++)
    {
        double distance = (m_path.row(i+1) - m_path.row(i)).norm();    // or .lpNorm<2>()
        double x1 = m_vel * m_vel / (2 * m_acc); 
        double x2 = distance - 2 * x1;
        double t1 = m_vel / m_acc;
        double t2 = x2 / m_vel;
        double delta = 2 * t1 + t2;
        for(int j = 1;j < m_p_num1d;++j)//不需要0次幂
        {
            m_time(i,j) = pow(delta,j);
            // m_time(i,j) = 1;
        }
    }
    // cout << m_time << endl;
}

void TimeAllocation::printTime()
{
    std::cout << "time matrix: " << endl << m_time << endl;
}